from pickletools import pyint
from turtle import right
import cv2
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.image as mpimg
import math

class Line_Detech:
    def __init__(self) -> None:
        
        self.prev_left_x1 = 0
        self.prev_left_x2 = 0
        self.prev_right_x1 = 0
        self.prev_right_x2 = 0
        # 左车道线平均斜率
        self.prev_left_avg_m = 0
        # 右车道线平均斜率
        self.prev_right_avg_m = 0
        # 左车道线截距
        self.prev_left_b = 0
        # 右车道线截距
        self.prev_right_b = 0
        self.prev_left_line = 0
        self.prev_right_line = 0
        # 探测完毕车道线后，是否需要显式出来
        self.show_image_flag = True
        # 显式过程处理图像标志位，gray，canny等处理图像显式
        self.show_processing_img_flag = False
        self.line_color = [255, 0, 0]
        
    def output(self, log_msg):
        print("**************************************************************")
        print(log_msg)
        
    def set_line_color(self, line_color):
        self.line_color = line_color
        
    def set_show_image_flag(self, flag):
        self.show_image_flag = flag

    def set_verices(self, left_down_p, left_top_p, right_top, right_down):
        # 指定识别车道线的范围(四角形, 多为梯形), 四个坐标位置: 左下, 左上, 右上, 右下

        # parameter for [images/lane2.jpeg]
        # vertices = np.array([[(0,255), (320, 150), (340, 150), (400, s[0])]], dtype=np.int32)

        # parameter for [images/lane8.jpeg]
        # vertices = np.array([[(0, 1000), (1125, 840), (1375, 840), (2500, s[0])]], dtype=np.int32)

        # parameter for [images/video_sample.jpg]
        # vertices = np.array([[(500, 2000), (2053, 1492), (2425, 1492), (4098, 1803)]], dtype=np.int32)
        
        self.vertices = np.array([[left_down_p, left_top_p, right_top, right_down]], dtype=np.int32)
        msg =  "## 梯形坐标 is: {}".format(self.vertices)
        self.output(msg)
        
    def show_image(self, image_path):
        img = mpimg.imread(image_path)
        self.output("image is: %s, with dimension: %s" % (str(type(img)), str(img.shape)))
        plt.imshow(img)
        plt.show()
        
    def set_line_angle(self, pLeft_line_angle, pRight_line_angle):
        '''
        左右车道线的斜率阈值, 对于左车道线需要小于指定阈值; 对于右车道线是要大于该阈值
        这里设置的原理就是通过设置一个左右车道要给大范围，在识别的时候，只是基于IOR区域上，检测指定
        角度范围的线，来做识别，
        
        通过以下角度尝试，可以推测出来角度范围越广， 识别出来的线的范围就越大，比如如果左车道线是10°，右车道线是170°，
        则可以可以识别出来完整的车道线，但是30~150°范围则只是识别出来左车道线，区间范围小于40~140°则完全识别不出来：
        170°/ 10°: 0.17 video_simple.jpeg可以识别出来中心两条线
        150°/ 30°: 0.57 video_simple.jpeg只能识别出来左车道线
        140°/ 40°: 0.87 video_simple.jpeg无法识别任何车道线
        130°/ 50°: 1.19 video_simple.jpeg无法识别任何车道线
        
        需要注意的是在opencv的视角下, 坐标轴是左上角, 所以需要将图像翻转过来才是我们通常的坐标系视角
        计算方法:
        1. 估算左车道线的角度,估算右车道的角度
        2. 以左车道20°,右车道50°为例, 左车道映射到cv坐标后的角度: 90° -20° + 90° = 160°，即真实角度的补角
        3. 将角度转化为斜率,推荐: https://www.calculator.io/zh/%E6%96%9C%E7%8E%87%E8%AE%A1%E7%AE%97%E5%99%A8/
           量角器：https://www.ginifab.com/feeds/angle_measurement/online_protractor.zh-cn.php
        4. 其中左侧到是160°, 对应的斜率是: -0.36;

        # parameter for [images/lane2.jpeg]
        left_estimated_slop = -0.83
        right_estimated_slop = -0.83

        # parameter for [images/lane8.jpeg]
        left_estimated_slop = -0.53
        right_estimated_slop = 0.27

        # parameter for [images/lane8.jpeg]
        left_estimated_slop = -0.27
        right_estimated_slop = 0.27
        '''
        self.left_estimated_slop = self.__caculate_line_slop__(pLeft_line_angle)
        self.right_estimated_slop = self.__caculate_line_slop__(pRight_line_angle)
        
        return self.left_estimated_slop, self.right_estimated_slop
    
    def detect_line(self, image_path, pExtrapolate_flag = True, show_processing_img_flag = False):
        self.detect_line_by_img_path(image_path, pExtrapolate_flag)
        
    def detect_line_by_img_path(self, image_path, pExtrapolate_flag = True, show_processing_img_flag = False):
        img = mpimg.imread(image_path)
        self.detect_line_by_imgobj(img, pExtrapolate_flag)
        
    def detect_line_by_imgobj(self, img, pExtrapolate_flag = True, show_processing_img_flag = False):
        s = img.shape

        img_gray = self.__grayscale__(img)
        # 对灰度处理后的图片进行去噪，即将二值化后的灰度图产生的“噪声白点”，进行消去
        img_blur = self.__gaussian_blur__(img_gray, kernel_size=5)
        # 进行边缘检测
        img_edge = self.__canny__(img_blur, low_threshhold=50, high_threshhold=150)
        # 基于指定区域（vertise）中，从img_edge中筛选出来目标边缘
        masked_edges = self.__region_of_interest__(img_edge, self.vertices)
        img_hough_lines = self.__hough_lines__(masked_edges, rho=1 , theta=np.pi/180, threshold=40, 
                                    min_line_len=60, max_line_gap=30, extrapolate=pExtrapolate_flag)
        img_lanes = self.__weighted_img__(img=img_hough_lines, initial_img=img, a=0.8, b=1)
        
        if show_processing_img_flag:
            plt.subplot(1, 4, 1)
            plt.imshow(img_gray)
            plt.title("1.gray picture")
            
            plt.subplot(1, 4, 2)
            plt.imshow(img_blur)
            plt.title("2.gauss blur")
            
            plt.subplot(1, 4, 3)
            plt.imshow(img_edge)
            plt.title("3.canny")
            
            plt.subplot(1, 4, 4)
            plt.imshow(masked_edges)
            plt.title("4.roi")
            print(img_edge.shape)
            plt.show()
            
        if self.show_image_flag == True:
            plt.subplot(1,1,1)
            plt.imshow(img_lanes)
            plt.title("5. draw lines")
            plt.show()
        else:
            pass
        
        return img_lanes

    def __grayscale__(self, img):
        '''
        灰度变换
        对于图像进行灰度处理，有以下几点考虑：
        1. 节约计算量，彩色图像是三通道（R、G、B），而灰度图只有单通道（0~255），所以处理图像效率高
        2. 边缘检测算法（Canny，Sobel）只需要亮度信息，并不需要彩色信息
        3. 统一亮度，提高鲁棒性，RGB图像中，不佟的颜色可能会干扰特征检测
        4. 方便后续二值化处理，注意灰度图和二值化处理并不一样，虽然两者都是单通道，但是灰度图取值范围是0~255，
            而二值化则是0，或者255
        '''
        # img来自于matplot.imread, 采用下面的枚举参数
        # 如果img来自于cv2.imread, 则采用下面的转换枚举(RGB vs. BGR)
        # return cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
        return cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)

    def __canny__(self, img, low_threshhold, high_threshhold):
        '''Cany边缘检测'''
        return cv2.Canny(img, low_threshhold, high_threshhold)

    def __gaussian_blur__(self, img, kernel_size):
        '''
        [高斯去噪]
        去噪主要用于去掉图像中噪声白点, 常见的去噪算法有高斯去噪,中值去噪, 其中高斯去噪应用的最广泛, 因为其性能和通用性都很好
        '''
        return cv2.GaussianBlur(img, (kernel_size, kernel_size), 0)

    def __median_blur__(self, img, kernel_size):
        ''' [中值去噪] '''
        return cv2.medianBlur(img, kernel_size)

    def __region_of_interest__(self, img, vertices):
        ''' 
        设置Canny算法处理区域, 用于缩小Cany发现边缘的范围, 指定区域外部分设置为黑色(255黑色, 0为白色
        vertices: 四个元素, 顺序以及意义: 左下, 左上, 右上, 左下
        '''
        # 构建一个和图片想同维度的多维数组
        mask = np.zeros_like(img)
        # 如果图像的维度大于2,则说明是x轴+y轴+颜色通道(类如RGB, 3通道);
        # 并且维度元组,最后一个元素才是描述通道数的
        # 这里设置蒙层颜色, 0是黑色, 255是白色, 这里的0~255可以立即为"亮度", 0的亮度最低, 255亮度最高, 所以代表白色
        if len(img.shape) > 2:
            # 获取颜色通道数量, 如果是RGB,则channel_count是3
            channel_count = img.shape[2]
            # (255,) * 3 => (255, 255, 255)
            ignore_mask_color = (255,) * channel_count
        # 如果shape维度等于2, 那么就是黑白/ 灰度图片, 如果没有颜色维度等价于颜色维度是1: (, , 1)
        # 即p[x][y] = 0, p[x][y] = 255
        else:
            ignore_mask_color = 255
        # 为蒙版在指定的区域(region_of_interest), 填充白色
        cv2.fillPoly(mask, vertices, ignore_mask_color)
        # 将图像和蒙版做融合, bitwise_and实现了感兴趣图像保持,不感兴趣部分全黑
        # 因为蒙版是感兴趣区域是白色,即255,全是1; 无关区域是黑色,即0; 当作and
        # 运算的时候, 根据"全1才是1",只要有一个是0就是就是0的原则, 实现了无关区域全0 (即黑色)
        masked_img = cv2.bitwise_and(img, mask)

        return masked_img

    
    def __draw_lines__(self, img, lines, color, thickness=2):
        '''
        画线段
        
        在OpenCV，颜色通道的顺序是“非”常规的：BGR，而在Matplot等图形处理软件中颜色通道顺序是常规的：RGB
        所以，对于颜色定义：color=[255, 0, 0]
        1. 如果使用maplot显示的图像是红色
        2. 如果使用OpenCV显示的蓝色
        '''
        for line in lines:
            for x1, y1, x2, y2 in line:
                cv2.line(img, (x1, y1), (x2, y2), color, thickness)

    def __get_y_intercept__(self, lane_lines, slopes):
        '''
        根据输入的一组线段和斜率，计算y方向的截距和平均斜率
        '''
        # 排除非法的斜率数据
        # 注: "~"代表排除
        slopes = slopes[~np.isnan(slopes)]
        slopes = slopes[~np.isinf(slopes)]
        # 计算斜率均值
        avg_slope = slopes.mean()
        # 将lines数据reshpae为[x,y]形式 (原始是[x1 y1 x2 y2]形式), 可以理解为将数据拉成两列形式,一列x值, 一列y值
        lane_lines = lane_lines.reshape((lane_lines.shape[0]*2, lane_lines.shape[1]//2))
        # axis=0代表纵向上求均值, 即求出x列的均值,以及y列均值
        # numpy的优美之处: 方便计算指定维度均值
        x_mean, y_mean = np.mean(lane_lines, axis=0)
        
        # 返回的是截距 (y = ax + b => b = y - ax), 以及平均斜率
        return y_mean - (x_mean * avg_slope), avg_slope

    def __get_x_intercepts__(self, y_1, y_2, slope, b):
        '''
        根据一个线段两端的y值, 以及线段的斜率和截距，计算对应的x值; 该函数用于"外推"逻辑中, 做线段联系使用
        '''
        msg = "get_x_intercepts::y_1: {}, y_2: {}, slope: {}, b: {}".format(y_1, y_2, slope, b)
        self.output(msg)    
        # if not (~np.isnan(slope) and ~np.isnan(b)) :
        if np.isnan(slope) or np.isnan(b):
            self.output("斜率非法! x值为0")
            x_1 = x_2 = 0.0
        else:
            x_1 = (y_1 - b)/slope
            x_2 = (y_2 - b)/slope
            self.output("x1_1: %f, x2_2: %f" % (x_1, x_2))

        return x_1, x_2

    def __draw_lines_extrapolated__(self, img, lines, color, thickness=10):
        imgshape = img.shape
        self.output("### draw_lines_extrapolated img.shape: {}".format(imgshape))
        # shape[0]以及[2]分别对应y1，y3
        # 原始shape是【18，1，4】，reshape之后就是【18，4】；第二个维度没什么用；推测是RGB中的一个通道
        # 对于lane1而言，一共是18个数据，Cany检测出来了是识别出了18段边缘
        lines = lines.reshape((lines.shape[0], lines.shape[2]))
        
        # "//"代表地板除法，即除法运算结果向下取整
        # 之前的【18，4】，其中18代表Cany检测中来的18段边缘; 4代表每个边缘的线段坐标: x1，y1, x2, y2;通过reshape转化为为了[36,2], 
        # 即将边缘(线段)的4维坐标, 转化为每个元素的"点"坐标(两个维度成员,分别代表x,y)
        # 其中 ":,1"说明如下:
        #     ":"代表第一个维度全选,即18*2个元素 (坐标) 全选, 其中":"等价于 "0:17"
        #     " ,"代表下一个维度
        #     "1"则代表下一个维度之选者索引为1的数据; 对于第二个维度其实有两个索引, 索引0代表x值, 索引1代表y值; 所以下面的式子就是要提取出来"y"列, 然后取最小
        # 注: ":"本身是"0:17"(m:n模式)的省略写法,代表获取当前维度的取值范围
        y_min = lines.reshape((lines.shape[0]*2, lines.shape[1]//2))[:,1].min()
        self.output("y_min: %f" % y_min)
        
        # slope是"斜率"的意思, 斜率是需要至少两个坐标参与计算才可以计算出来; 计算方法就是: 
        # slop = (y2 - y1) /(x2 - x1)
        # 下面的式子就是将所有的元素(18个元素)的x和y两两相减来计算斜率 (其中, 3和1对应的y2, y1, 2和0对应的x2和x1) 
        # 这个就是numpy的优雅实现, 不需要for语句, 只需要了解数组和维度的使用, 一个式子就可以解决问题
        # 每个lines是有四个属性，或者说四个维度，line[:, 3]代表第四个属性/维度，取全部的数据
        # 注: 这里的斜率是y方向的斜率, 即y轴方向的斜率, 所以斜率是y方向的斜率, 斜率绝对值是y方向的斜率绝对值
        slopes = (lines[:, 3] - lines[:, 1])/(lines[:, 2] - lines[:, 0])
        
        # 去除掉非法的斜率值 (nan值以及无穷大)的lines
        # 如果slopes进行了去除inf值，lines一定要同步进行该操作，因为后面的有lines和slope结合操作的代码：
        #
        # left_lines = lines[slopes < left_estimated_slop]
        # 
        # 所以，两者务必要保持维度一致
        lines = lines[~np.isinf(slopes)]
        # 去除掉非法的斜率值 (nan值以及无穷大)
        slopes = slopes[~np.isinf(slopes)]
        slopes = slopes[~np.isnan(slopes)]
        
        # 左侧行车道的斜率为负值，斜率绝对值是一个经验值, 需要根据真实的拍摄角度, 映射的角度来判断
        # 所以该算法主要适合于摄像头角度固定场景
        # opencv是以左上角为坐标系0原点，所以，斜率正负是以此为原点计算，故正负值和直觉相反
        # 注: lines[slopes < -0.5]中的"slopes < -0.5"获取的是slopes小于-0.5的索引, 根据这个索引再来到lines里面获取对应数据
        # 这张写法要求lines和slopes的数据是一一对应关系
        left_lines = lines[slopes < self.left_estimated_slop]
        right_lines = lines[slopes > self.right_estimated_slop]
        left_slopes = slopes[slopes < self.left_estimated_slop]
        right_slopes = slopes[slopes > self.right_estimated_slop]

        print("draw_lines_extrapolated::left_lines: {}".format(left_lines))
        print("draw_lines_extrapolated::left_slopes: {}".format(left_slopes))
        print("draw_lines_extrapolated::right_slopes: {}".format(right_slopes))
        # 获得左/ 右车道线截距以及斜率
        left_b, left_avg_m = self.__get_y_intercept__(left_lines, left_slopes)
        right_b, right_avg_m = self.__get_y_intercept__(right_lines, right_slopes)
        print("draw_lines_extrapolated::left_b: %f" % left_b)
        print("draw_lines_extrapolated::left_avg_m: %f" % left_avg_m)
        print("draw_lines_extrapolated::right_b: %f" % left_b)
        print("draw_lines_extrapolated::right_avg_m: %f" % right_avg_m)
        keep_prev_left = False
        keep_prev_right = False
        # 对于异常情况, 如果此次的均值结果偏差比较大,则在有历史数据的情况下, 采用历史数据"进行纠正"
        # 下面处理主要用于连续多次处理图像场景, 比如分析视频连续多帧场景
        if (self.prev_left_avg_m!=0) and (left_avg_m < -0.98 or left_avg_m > -0.36):
            left_avg_m = self.prev_left_avg_m
            left_b = self.prev_left_b
            keep_prev_left = True

        if (self.prev_left_avg_m != 0) and (right_avg_m > 0.98 or right_avg_m < 0.36):
            right_avg_m = self.prev_left_avg_m
            right_b = self.prev_right_b
            keep_prev_right = True
        
        # 记录最终设定值 (可能是原始值, 也可能是纠正之后的值) 作为下一次的异常数据处理
        self.prev_left_avg_m = left_avg_m
        self.prev_right_avg_m = right_avg_m
        self.prev_left_b = left_b
        self.prev_right_b = right_b
        
        # 根据本次计算的斜率和截距，计算对应的y值的x
        left_x1, left_x2 = self.__get_x_intercepts__(y_1=y_min, y_2=imgshape[0], slope=left_avg_m, b=left_b)
        right_x1, right_x2 = self.__get_x_intercepts__(y_1=y_min, y_2=imgshape[0], slope=right_avg_m, b=right_b)
        print("draw_lines_extrapolated:left_x1: %f" % left_x1)
        print("draw_lines_extrapolated:left_x2: %f" % left_x1)
        print("draw_lines_extrapolated:right_x1: %f" % right_x1)
        print("draw_lines_extrapolated:right_x2: %f" % right_x2)
        
        ## 求累积平均值 ##
        if self.prev_left_x1 != 0 or self.prev_left_x2 !=0 or self.prev_right_x1 != 0 or self.prev_right_x2 != 0:
            alpha = 0.2
            left_x1_new = math.floor((alpha) * left_x1 + (1-alpha)*self.prev_left_x1)
            left_x2_new = math.floor((alpha) * left_x2 + (1-alpha)*self.prev_left_x2)
            right_x1_new = math.floor((alpha) * right_x1 + (1-alpha)*self.prev_right_x1)
            right_x2_new = math.floor((alpha) * right_x2 + (1-alpha)*self.prev_right_x2)
            self.prev_left_x1 = left_x1_new
            self.prev_left_x2 = left_x2_new
            self.prev_right_x1 = right_x1_new
            self.prev_right_x2 = right_x2_new
        else:
            left_x1_new = left_x1
            left_x2_new = left_x2
            right_x1_new = right_x1
            right_x2_new = right_x2
        print("draw_lines_extrapolated:left_x1_new: %f" % left_x1_new)
        print("draw_lines_extrapolated:y_min: %f" % y_min)
        print("draw_lines_extrapolated:left_x2_new")
        print(left_x2_new)
        print("draw_lines_extrapolated:imgshape[0]")
        print(imgshape[0])
        left_line = np.array([left_x1_new, y_min, left_x2_new, imgshape[0]], dtype=np.int32)
        right_line = np.array([right_x1_new, y_min, right_x2_new, imgshape[0]], dtype=np.int32)

        if keep_prev_left:
            left_line = self.prev_left_line
            left_x1_new = self.prev_left_x1
            left_x2_new = self.prev_left_x2
        if keep_prev_right:
            right_line = self.prev_right_line
            right_x1_new = self.prev_right_x1
            right_x2_new = self.prev_right_x2
        
        # 描画左车道线
        cv2.line(img, (int(left_x1_new), int(y_min)), (int(left_x2_new), imgshape[0]), color, thickness)
        # 描画右车道线
        cv2.line(img, (int(right_x1_new), int(y_min)), (int(right_x2_new), imgshape[0]), color, thickness)


    def __hough_lines__(self, img, rho, theta, threshold, min_line_len, max_line_gap, extrapolate=False):
        '''
        img: canny边缘检测输出
        rho：Houph Grid中像素的距离
        theta：Hough grid中角度距离
        threshold：最小的voting数值
        min_line_len：构成一条线的最小像素数个数
        max_line_gap：可以相连的线段之间的最大的距离
        返回黑背景上面有白线的图
        '''
        # 霍夫曼算法(因为结尾是"P",所以是霍夫曼的优化改进算法)检测指定图片 (已经被二值化且Canny处理过的图像) 线段
        lines = cv2.HoughLinesP(img, rho, theta, threshold, np.array([]) , minLineLength=min_line_len, maxLineGap=max_line_gap)
        # 获得一张和原始图片大小一样的黑色背景图片
        line_img = np.zeros((img.shape[0], img.shape[1], 3), dtype=np.uint8)
        # 在该褐色背景图片中描绘检测出来的线段
        # 如果不需要外推, 只是把识别出来的线段显示出来即可
        if not extrapolate:
            self.__draw_lines__(line_img, lines, self.line_color)
        # 需要外推, 则是根据线段, 将左右车道线以两条直线形式画出来
        else:
            self.__draw_lines_extrapolated__(line_img, lines, self.line_color)

        # 返回黑底白线的图像
        return line_img


    def __weighted_img__(self, img, initial_img, a=0.8, b=1.):
        '''
        img: hougp_lines()的输出,黑背景上面有白线的图
        initial_img: 原始图像
        加权相加:
        initial_img * a + img * b
        '''
        # 将hougp_lines()的输出图像和原始图像做加权叠加, 效果就是
        return cv2.addWeighted(initial_img, a, img, b, 0)

    def __angle_to_slope__(self, angle_degrees):
        """ 根据角度计算斜率 """
        angle_radians = math.radians(angle_degrees)  # 角度转弧度
        return math.tan(angle_radians)  # 计算斜率
    
    def __caculate_line_slop__(self, degree):
        vc2_slop_degree = 90 - degree + 90
        line_slope = self.__angle_to_slope__(vc2_slop_degree)
        
        return line_slope

# # 测试示例
# angle = 30
# slope = angle_to_slope(angle)
# print(f"角度 {angle}° 对应的斜率是: {slope}")
def main():
    image_path = "images/video_sample.jpg"
    img = mpimg.imread(image_path)
    ld = Line_Detech()
    ld.set_verices((0, 1750), (500, 1500), (3500, 1500), (4000, 1750))
    ld.set_line_angle(20, 160)
    ld.set_show_image_flag(True)
    
    ld.detect_line(image_path, True)
    # ld.detect_line_by_img(img)
    
def test_slope():
    left_angle = 30
    right_angle = 150
    ld = Line_Detech()
    left_slope, right_slope = ld.set_line_angle(30, 150)
    print("left: %f right: %f" % (left_slope, right_slope))
    
if __name__ == "__main__":
    main()
    # show_image("images/video_sample.jpg")
    # test_slope()


